//
// Created by lly on 2024/6/16.
//
#ifndef WI_FI_F429_TASK_SCHEDULING_L_H
#define WI_FI_F429_TASK_SCHEDULING_L_H

#include "stm32h723xx.h"
#include "stdbool.h"
#include "cmsis_os2.h"

#define id_vesc_shoot   93
#define id_ari_board  0x02

typedef enum {
    free_time = 0,
    belly_receball = 1,
    front_receball = 2,
    claw_grain = 3,
    lay_grain = 4,
    accelerate_shoot = 5,
    decelerate_shoot = 6,
    shoot_ball = 7,
    game_begin = 8,
    ready_shoot=9,
} Task_status;

/////////////////////////////Motor_control//////////////////////////////////////////////////////////////
#define claw_pick_ball_grasp_dji2006_pos_close (-29.f)       ///claw_pick_ball_grasp_dji2006_pos
#define claw_pick_ball_grasp_dji2006_pos_open (-150.f)

#define claw_pick_ball_overturn_dji2006_pos_init 200     ///claw_pick_ball_overturn_dji2006_pos
#define claw_pick_ball_overturn_dji2006_pos_belly 0u
#define claw_pick_ball_overturn_dji2006_pos_front 490

#define shoot_ball_front_vesc_vel_change 15000             //oot_ball_front_vesc_vel

#define shoot_ball_behind_dji2006_vel_shoot_belly (5000.f)     ///front -3000         //oot_ball_behind_dji2006_vel
#define shoot_ball_behind_dji2006_vel_shoot (5000.f)     ///front -3000         //oot_ball_behind_dji2006_vel

#define claw_grain_dji3508_pos_fetch_l 150.f                 ///claw_grain_dji3508_pos
#define claw_grain_dji3508_pos_fetch_r (-150.f)
#define claw_grain_dji3508_pos_fetch_lay_l 150.f                 ///claw_grain_dji3508_pos
#define claw_grain_dji3508_pos_fetch_lay_r (-155.f)                 ///claw_grain_dji3508_pos
#define claw_grain_dji3508_pos_lay_l (-10)
#define claw_grain_dji3508_pos_lay_r 10

#define claw_storage_dji2006_pos_fetch_l (12)               ///claw_storage_left_dji2006_pos
#define claw_storage_dji2006_pos_fetch_r (-10)               ///claw_storage_left_dji2006_pos
#define claw_storage_dji2006_pos_lay_l (-105.f)
#define claw_storage_dji2006_pos_lay_r (95.f)

typedef struct {
    float claw_pick_ball_grasp_dji2006_pos;      ///Slide grip
    float claw_pick_ball_overturn_dji2006_pos;   ///claw overturn
    int32_t shoot_ball_front_vesc_vel;
    float shoot_ball_behind_dji2006_vel;
    float claw_grain_left_dji3508_pos;
    float claw_grain_right_dji3508_pos;
    float claw_storage_left_dji2006_pos;
    float claw_storage_right_dji2006_pos;
} Motor_Vel_Pos;
///////////////////////////////////////////////////////////////////////////////////////////////////////////////

/////////////////////////////time_stamp//////////////////////////////////////////////////////////////

typedef struct {
    bool flag_left_had_claw_grain;    /// The time between catching and lifting
    bool begin_keeptime_left_claw_grain;
    uint32_t time_left_begin_claw_grain;

    bool flag_is_Eliminate_speedacce_jitter;
    bool begin_keeptime_from_last_acceespeed;
    uint32_t time_from_last_accespeed;

    bool flag_is_Eliminate_speeddcce_jitter;
    bool begin_keeptime_from_last_dcceespeed;
    uint32_t time_from_last_dccespeed;
} Time_Stamp;
////////////////////////////////////////////////////////////////////////////////////////////////////////////

/////////////////////////////air_cylinder_control//////////////////////////////////////////////////////////////

#define id_air_cylinder_claw_1   1
#define id_air_cylinder_claw_2   2
#define id_air_cylinder_claw_3   3
#define id_air_cylinder_updown_catch_grain 4
#define open_AC  1
#define close_AC 0
typedef struct {
    bool air_cylinder_stretch_claw_1_l;     ///open ץ
    bool air_cylinder_stretch_claw_1_r;     ///open ץ
    bool air_cylinder_stretch_claw_2;
    bool air_cylinder_stretch_claw_3;
    bool air_cylinder_updown_catch_grain; ///open ��
} Air_Param;
typedef enum {
    air_cylinder_stretch_claw_1_l=2,
    air_cylinder_stretch_claw_1_r=1,
    air_cylinder_stretch_claw_2=4,
    air_cylinder_stretch_claw_3=5,
    air_cylinder_updown_catch_grain=0,
};
////////////////////////////////////////////////////////////////////////////////////////////////////////////

#define control_mode_auto 1
#define control_mode_handle 2

#define task_state_handle_step_null 0
#define task_state_handle_step1 1
#define task_state_handle_step2 2
#define task_state_handle_step3 3
#define task_state_handle_step4 4
typedef struct {
    uint8_t task_state;
    uint8_t control_mode;
    uint8_t task_state_handle;
} Main_control;

typedef struct {
    bool erupt_A;
    bool erupt_B;
    bool erupt_C;
    bool erupt_D;
} Erupt_;

extern Erupt_ Erupt;
extern Time_Stamp Time_stamp;
extern Motor_Vel_Pos Motor_param_set;
extern Main_control Main_Control;
extern Air_Param Air_para;

void Air_Cylinder_Control_blue();
void Air_Cylinder_Control_red();

void usb_receive(uint8_t *Buf);

void time_stamp();

void Task_change();

void windows_cb();
void serial_init();
#define Wait  osDelay
#endif //WI_FI_F429_TASK_SCHEDULING_L_H
